package controller;

import java.io.IOException;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.DifferentialPilot;
import model.RobotModel;

public class Controller {
	
	private static final String MSG_ERROR = "Erro: ";
	private static final String WHITE_SPACE = " ";
	private static final String ROTATE_SPEED_COMMAND = "VER";
	private static final String DISCONNECT_COMMAND = "DIS";
	private static final String END = "END";
	private static final String TRAVEL_SPEED_COMMAND = "VEL";
	private static final String TURN_RIGHT_COMMAND = "DIR";
	private static final String TURN_LEFT_COMMAND = "ESQ";
	private static final String SPIN_COMMAND = "GIR";
	private static final String BACK_COMMAND = "VOL";
	private static final String STOP_COMMAND = "PAR";
	private static final String MEASURE_COMMAND = "MED";
	private static final String WALK_COMMAND = "AND";
	private float INITIAL_TRAVEL_SPEED = 1;  // travel speed in wheel diameter units per second
	private float INITIAL_ROTATE_SPEED = 10;  // rotation speed of the vehicle, degrees per second

	private DifferentialPilot pilot;
	private UltrasonicSensor sensor;
	private BluetoothController bluetoothController;
	private static int line = 0;
	private boolean waiting_connection;
	
	private RobotModel robotModel;
	
	public Controller() {
		robotModel = new RobotModel();
		pilot = new DifferentialPilot(RobotModel.WHEEL_RADIUS, RobotModel.WHEELBASE, Motor.A, Motor.B);
		sensor = new UltrasonicSensor(SensorPort.S4);
		
		pilot.setTravelSpeed(INITIAL_TRAVEL_SPEED);
		pilot.setRotateSpeed(INITIAL_ROTATE_SPEED);
		
	}
	
	public static void cleanScreen() {
		LCD.clear();
		line = 0;
	}
	
	public static void writeToScreen(final String text) {
		LCD.drawString(text, 0, line);
		++line;
	}
	
	private static boolean findSubStr(char[] str, char[] subStr) {
		if (str.length < subStr.length) {
			return false;
		}
		for (int i = 0; i < subStr.length; i++) {
			if (str[i] != subStr[i]) {
				return false;
			}
		}
		return true;
	}
	
	private int measureCommand() {
		int distancia = sensor.getDistance();
		
		try {
			bluetoothController.sendMessage(distancia);
			writeToScreen(MEASURE_COMMAND + WHITE_SPACE + distancia);
		} catch (IOException e) {
			writeToScreen(MSG_ERROR + MEASURE_COMMAND);
		}
		
		return distancia;
	}
	
	private void walkCommand() throws IOException {
		int walk = bluetoothController.getMessageParam();
		pilot.travel(walk);
		float distance = pilot.getMovement().getDistanceTraveled();
		bluetoothController.sendMessage(distance);
		writeToScreen(WALK_COMMAND + WHITE_SPACE + distance);
	}
	
	private void travelSpeedCommand() throws IOException {
		robotModel.setTravelSpeed(bluetoothController.getMessageParam());
		pilot.setTravelSpeed(robotModel.getTravelSpeed());
		writeToScreen(TRAVEL_SPEED_COMMAND + WHITE_SPACE + robotModel.getTravelSpeed());
	}
	
	private void rotateSpeedCommand() throws IOException {
		robotModel.setRotateSpeed(bluetoothController.getMessageParam());
		pilot.setRotateSpeed(robotModel.getRotateSpeed());
		writeToScreen(ROTATE_SPEED_COMMAND + WHITE_SPACE + robotModel.getRotateSpeed());
	}
	
	private void spinCommand() throws IOException {
		int angle = bluetoothController.getMessageParam();
		pilot.rotate(angle);
		float spun = pilot.getMovement().getAngleTurned();
		bluetoothController.sendMessage(spun);
		writeToScreen(SPIN_COMMAND + WHITE_SPACE + spun);
	}
	
	/**
	 * If radius is positive, the robot arcs left, and the center of the turning circle is on the left side of the robot.
     * If radius is negative, the robot arcs right, and the center of the turning circle is on the right side of the robot.
     * If radius is zero, is zero, the robot rotates in place.
     *  
	 * @throws IOException
	 */
	private void leftTurnCommand() throws IOException {
		int angle = bluetoothController.getMessageParam();		
		pilot.arc(RobotModel.WHEELBASE/2, angle);
		
		float steered = pilot.getMovement().getArcRadius();
		bluetoothController.sendMessage(steered);
		writeToScreen(TURN_LEFT_COMMAND + WHITE_SPACE + steered);
	}
	
	/**
	 * If radius is positive, the robot arcs left, and the center of the turning circle is on the left side of the robot.
     * If radius is negative, the robot arcs right, and the center of the turning circle is on the right side of the robot.
     * If radius is zero, is zero, the robot rotates in place.
     *  
	 * @throws IOException
	 */
	private void rightTurnCommand() throws IOException {
		int angle = bluetoothController.getMessageParam();
		pilot.arc(-RobotModel.WHEELBASE/2, angle);

		float steered = pilot.getMovement().getArcRadius();
		bluetoothController.sendMessage(steered);
		writeToScreen(TURN_RIGHT_COMMAND + WHITE_SPACE + steered);
	}
	
	private void backCommand() throws IOException {
		int walk = bluetoothController.getMessageParam();
		pilot.travel(-walk);

		float distance = pilot.getMovement().getDistanceTraveled();
		bluetoothController.sendMessage(distance);
		writeToScreen(BACK_COMMAND + WHITE_SPACE + distance);
	}
	
	private void runCommands(char[] cmd) throws IOException {
		
		if (findSubStr(cmd,WALK_COMMAND.toCharArray())) {
			walkCommand();
		}
		else if (findSubStr(cmd,MEASURE_COMMAND.toCharArray())) {
			measureCommand();
		}
		else if (findSubStr(cmd,STOP_COMMAND.toCharArray())) {
			// TODO preencher
			writeToScreen(STOP_COMMAND);
			int time = bluetoothController.getMessageParam();
		}
		else if (findSubStr(cmd,BACK_COMMAND.toCharArray())) {
		}
		else if (findSubStr(cmd,SPIN_COMMAND.toCharArray())) {
			spinCommand();
		}
		else if (findSubStr(cmd,TURN_LEFT_COMMAND.toCharArray())) {
			leftTurnCommand();
		}        
		else if (findSubStr(cmd,TURN_RIGHT_COMMAND.toCharArray())) {
			rightTurnCommand();
		}
		else if (findSubStr(cmd,TRAVEL_SPEED_COMMAND.toCharArray())) {
			travelSpeedCommand();
		}
		else if (findSubStr(cmd,ROTATE_SPEED_COMMAND.toCharArray())) {
			rotateSpeedCommand();
		}  
		else if (findSubStr(cmd,END.toCharArray())) {
			// TODO preencher
			writeToScreen(END);
		}
		else if (findSubStr(cmd,DISCONNECT_COMMAND.toCharArray())) {
			writeToScreen(DISCONNECT_COMMAND);
			bluetoothController.disconnect();
			waiting_connection = true;
		} else {
			writeToScreen("ops");
		}
	}
	
	public void main() {
		waiting_connection = true;
		bluetoothController = new BluetoothController();
		
		while(!Button.ESCAPE.isPressed()) {
			if (waiting_connection) {
				
				bluetoothController.connect();
				waiting_connection = false;
			}
			
			char[] cmd;		
			
			try {
				cmd = bluetoothController.getMessage();
				
				cleanScreen();
				runCommands(cmd);
			} catch (IOException e) {
				e.printStackTrace();
			}
			
		}
		
		bluetoothController.disconnect();
	}
}
